//
// Copyright (c) Microsoft Corporation.  All rights reserved.
//
//
// Use of this sample source code is subject to the terms of the Microsoft
// license agreement under which you licensed this sample source code. If
// you did not accept the terms of the license agreement, you are not
// authorized to use this sample source code. For the terms of the license,
// please see the license agreement between you and Microsoft or, if applicable,
// see the LICENSE.RTF on your install media or the root of your tools installation.
// THE SAMPLE SOURCE CODE IS PROVIDED "AS IS", WITH NO WARRANTIES OR INDEMNITIES.
//
//
// (C) Copyright 2006 Marvell International Ltd.
// All Rights Reserved
//

#include <windows.h>
#include <ceddk.h>

#include "Cs.h"
#include "Csmedia.h"

#include "CameraPDDProps.h"
#include "dstruct.h"
#include "dbgsettings.h"
#include <camera.h>
#include "CameraDriver.h"
#include "PinDriver.h"
#include "SensorFormats.h"

#include "monahans.h"
#include "Ov5623.h"
#include "xllp_Ov5623_hw.h"
#include "xllp_camera.h"
#include "xllp_ost.h"
#include "xllp_i2c.h"
#include "xllp_gpio_plat.h"
#include "GPX_XLLP.h"
#include "micco.h"
#include "xllp_camera_os_depend.h"

#include "PVRJpeg.h"

extern "C" unsigned int gpio_reg;
extern "C" unsigned int ost_reg;
extern "C" unsigned int i2c_reg;

const int QXGA_WIDTH  = 2048;
const int QXGA_HEIGHT = 1536;
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_CIF_YV12, 352, -288, 12, 15);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_SVGA_YV12, 800, -600, 12, 30);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_SXGA_YV12, 1280, -960, 12, 15);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_UXGA_YV12, 1600, -1200, 12, 15);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_QXGA_YV12, QXGA_WIDTH, -QXGA_HEIGHT, 12, 3);
MAKE_STREAM_MODE_YV12(DCAM_StreamMode_FULL_YV12, 2560, -1920, 12, 3);

MAKE_STREAM_MODE_IJPG(DCAM_StreamMode_QXGA_IJPG, 2048, 1536, 16, 3);

UCHAR get_analog_gain(USHORT gain);
void get_digi_gain(USHORT gain, UCHAR* digi_gain1, UCHAR* digi_gain2);

static color_cfg_t color_cfg = 
{
    0,
    0,
    { 
        1.036, 0.097, -0.133,
        -0.006, 1.217, -0.211,
        0.012, -0.386, 1.374
    }
};

static camera_cfg_t preview_cfg = 
{
    &DCAM_StreamMode_CIF_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_RAW10,     /* sensor format */
    CIF,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_RAW10,
        352, 288
    },
    &color_cfg
};

static camera_cfg_t still_svga_cfg = 
{
    &DCAM_StreamMode_SVGA_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_RAW10,     /* sensor format */
    SVGA,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR,
        800, 600
    },
    &color_cfg
};

static camera_cfg_t still_cfg = 
{
    &DCAM_StreamMode_UXGA_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_RAW10,     /* sensor format */
    UXGA,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_RAW10,
        1600, 1200
    },
    &color_cfg
};

static camera_cfg_t still_sxga_cfg = 
{
    &DCAM_StreamMode_SXGA_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_RAW10,     /* sensor format */
    SXGA,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_RAW10,
        1280, 960
    },
    &color_cfg
};

static camera_cfg_t still_full_cfg = 
{
    &DCAM_StreamMode_FULL_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_RAW10,     /* sensor format */
    FULL,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_RAW10,
        2560, 1920
    },
    &color_cfg
};

static camera_cfg_t still_ijpg_cfg = 
{
    &DCAM_StreamMode_QXGA_IJPG,
    XLLP_CAMERA_IMAGE_FORMAT_RAW10,     /* sensor format */
    QXGA,
	/* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_RAW10,
        2048, 1536
    },
	&color_cfg
};

static camera_cfg_t still_qxga_cfg = 
{
    &DCAM_StreamMode_FULL_YV12,
    XLLP_CAMERA_IMAGE_FORMAT_RAW10,     /* sensor format */
    QXGA,
    /* qci_format_t */
    {
        XLLP_CAMERA_IMAGE_FORMAT_RAW10,
        QXGA_WIDTH, QXGA_HEIGHT
    },
    &color_cfg
};

static qci_interface_t ov5623_interface =
{
    XLLP_CI_MODE_MP,
    XLLP_CI_DATA_WIDTH10,
    1300,
    XLLP_FALSE, 
    XLLP_FALSE, 
    XLLP_FALSE
};

static unsigned char lut[] = {
    // RED LUT
    0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, 0x18, 0x1c,
    0x20, 0x24, 0x28, 0x2c, 0x30, 0x34, 0x38, 0x3c,
    0x40, 0x44, 0x48, 0x4c, 0x50, 0x54, 0x58, 0x5c,
    0x60, 0x64, 0x68, 0x6c, 0x70, 0x74, 0x78, 0x7c,
    0x80, 0x84, 0x88, 0x8c, 0x90, 0x94, 0x98, 0x9c,
    0xa0, 0xa4, 0xa8, 0xac, 0xb0, 0xb4, 0xb8, 0xbc,
    0xc0, 0xc4, 0xc8, 0xcc, 0xd0, 0xd4, 0xd8, 0xdc,
    0xe0, 0xe4, 0xe8, 0xec, 0xf0, 0xf4, 0xf8, 0xfc,

    // BLUE LUT
    0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, 0x18, 0x1c,
    0x20, 0x24, 0x28, 0x2c, 0x30, 0x34, 0x38, 0x3c,
    0x40, 0x44, 0x48, 0x4c, 0x50, 0x54, 0x58, 0x5c,
    0x60, 0x64, 0x68, 0x6c, 0x70, 0x74, 0x78, 0x7c,
    0x80, 0x84, 0x88, 0x8c, 0x90, 0x94, 0x98, 0x9c,
    0xa0, 0xa4, 0xa8, 0xac, 0xb0, 0xb4, 0xb8, 0xbc,
    0xc0, 0xc4, 0xc8, 0xcc, 0xd0, 0xd4, 0xd8, 0xdc,
    0xe0, 0xe4, 0xe8, 0xec, 0xf0, 0xf4, 0xf8, 0xfc,

    // GREEN LUT
    0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, 0x18, 0x1c,
    0x20, 0x24, 0x28, 0x2c, 0x30, 0x34, 0x38, 0x3c,
    0x40, 0x44, 0x48, 0x4c, 0x50, 0x54, 0x58, 0x5c,
    0x60, 0x64, 0x68, 0x6c, 0x70, 0x74, 0x78, 0x7c,
    0x80, 0x84, 0x88, 0x8c, 0x90, 0x94, 0x98, 0x9c,
    0xa0, 0xa4, 0xa8, 0xac, 0xb0, 0xb4, 0xb8, 0xbc,
    0xc0, 0xc4, 0xc8, 0xcc, 0xd0, 0xd4, 0xd8, 0xdc,
    0xe0, 0xe4, 0xe8, 0xec, 0xf0, 0xf4, 0xf8, 0xfc,
};

#define RGGB_MCU_HEIGHT     16

#define SENSOR_INIT_WAIT  150
void map_registers();
static void dump_Ov5623();

static IppiRawPixProcSpec_P3R* rggb2yuv_init(unsigned int width,
                                      unsigned int height,
                                      float* coe);
static BOOL rggb2yuv_convert(PUCHAR* dest, frame_t* frame, IppiRawPixProcSpec_P3R *ippiRPPSpec);
static void rggb2yuv_deinit(IppiRawPixProcSpec_P3R *ippiRPPSpec);
extern void fill_buffer_yv12(PUCHAR* dest_plane, frame_t* frame);
Ov5623::Ov5623()
{
    map_registers();
    qci_interface = &ov5623_interface;
    timing.BFW = 0x01;
    timing.BLW = 0x01;   /*GRBG to RGGB*/  

    rpp_uxga = rggb2yuv_init(1600, 1200, color_cfg.color_correct_coe);
    rpp_qxga = rggb2yuv_init(QXGA_WIDTH, QXGA_HEIGHT, color_cfg.color_correct_coe);
    rpp_cif = rggb2yuv_init(352, 288, color_cfg.color_correct_coe);
    rpp_full = rggb2yuv_init(2560, 1920, color_cfg.color_correct_coe);
    rpp_mcu = rggb2yuv_init(2048, RGGB_MCU_HEIGHT * 16, color_cfg.color_correct_coe); //
}

bool Ov5623::detect()
{
    XLLP_UINT8_T pid = 0, rev = 0;

    OV5623VersionRevision(&pid, &rev);
    NKDbgPrintfW(L"5623 sensor pid %d, rev %d", pid, rev);

    return pid == PIDH_OV5623 && rev == PIDL_OV5623;
}

void Ov5623::set_disable()
{
    XLLP_UINT8_T buf = 0;
    OV5623WriteSensorReg(OV5623_COM4, &buf);
    RETAILMSG(1, (TEXT("Disable ov5623 now.\r\n")));
}

unsigned int Ov5623::get_formats(ULONG type, PCS_DATARANGE_VIDEO** formats)
{
    // Video Format initialization
    unsigned int nformats = 1;

    *formats = new PCS_DATARANGE_VIDEO[nformats];

    if( NULL == *formats )
    {
        return 0;
    }

    switch (type)
    {
    case CAPTURE:
    case PREVIEW:
        (*formats)[0] = &DCAM_StreamMode_CIF_YV12;
        break;
    case STILL:
        //(*formats)[0] = &DCAM_StreamMode_SXGA_YV12;
        //(*formats)[0] = &DCAM_StreamMode_UXGA_YV12;
        //(*formats)[0] = &DCAM_StreamMode_FULL_YV12;
        //(*formats)[0] = &DCAM_StreamMode_QXGA_YV12;
        (*formats)[0] = &DCAM_StreamMode_QXGA_IJPG;
        break;
    default:
        break;
    }
    return nformats;
}

void Ov5623::start_capture()
{
    USHORT exposure = 250 * 2;
    USHORT gain = 4 * 2;

    if (frame_size == UXGA)
    {
        exposure *= 2;
    }

    if (frame_size == CIF) 
    {
        // Turn on auto function on preview mode.
        OV5623AutoFunctionOn();
    }
    else
    {
        OV5623AutoFunctionOff();        
        //set_exposure_gain(exposure, gain);
        calculate_exp();
    }
    
    OV5623ViewfinderOn();    
    // dump_Ov5623();
}

void Ov5623::stop_capture()
{
    field_protect();
    OV5623ViewfinderOff();    
}

extern BOOL pic_read_reg(HANDLE pic_drv, UCHAR addr, UCHAR* value);
extern BOOL pic_write_reg(HANDLE pic_drv, UCHAR addr, UCHAR* value);
static void set_voltage(UCHAR camera_ana, UCHAR camera_io, UCHAR camera_core, BOOL is_on)
{
   /* HANDLE pic_drv;
    
    // camera_ana is from V_CAM_2P8V, which is from V_TECH_3V.
    const UCHAR V_3V_1P8V_REG = 0x13;  // LDO 3, 4.
    const UCHAR V_TECH_3V_1P8V_REG = 0x15;  // LDO 10, 11.
    //const UCHAR V_CAM_ANA_MASK = 0xE0;
    
    // camera_io is from V_CAM_1P8V. which is from V_TECH_1P8V.
    
    // camera_core is from V_CAM_1P3V, which is from V_BATT.
    
    camera_ana <<= 5;
    camera_io  <<= 4;    

    pic_drv = CreateFile(PIC_FILE_NAME, 0, 0, NULL, 0, 0, NULL);

    if (pic_drv == INVALID_HANDLE_VALUE) {
        NKDbgPrintfW(L"camera: open pic file failed!\r\n");
    } else {
        UCHAR reg;
        
        pic_read_reg(pic_drv, V_3V_1P8V_REG, &reg);
        NKDbgPrintfW(L"V_3V_1P8V_REG = 0x%02X.\r\n", reg);
        //if ((reg & V_CAM_ANA_MASK) != camera_ana) {
        //    pic_write_reg(pic_drv, V_CAM_ANA_REG, (reg & ~V_CAM_ANA_MASK) | camera_ana);
        //}
        
        pic_read_reg(pic_drv, V_TECH_3V_1P8V_REG, &reg);
        NKDbgPrintfW(L"V_TECH_3V_1P8V_REG = 0x%02X.\r\n", reg);
        //if ((reg &  V_CI_MASK) != camera_io) {
        //    pic_write_reg(pic_drv, V_CI_REG, (reg &  ~V_CI_MASK) | camera_io);
        //}

        CloseHandle(pic_drv);
    }*/
}

void Ov5623::set_power_mode(bool is_on)
{
    // All camera voltage is from V_BATT.  No need to set.
    set_voltage(0xa, 0xa, 0x0, is_on);
#if 0
    /* Call_GPX */
    GPX_SetDirection(XLLP_GPIO_CAMERA_HI_PWDN, XLLP_GPIO_DIRECTION_OUT);
#endif

    //---------------------------------------------------------------------------------------
    // GPIO Expander address is 0xC6/0XC7.  Means slave address is 0x63.
    //0  PD_N            Wifi/BT is not in Power Down mode
    //1  RESET_N            Wifi/BT is not in Reset
    //2  CIF_PWDN        Camera is in Power Down mode
    //3  CIF_RESET_N        Camera is not in Reset
    //4  REV_ID<0>        Torch and Flash mode disabled
    //5  REV_ID<1>
    //6  REV_ID<2>
    //7  TECH_INT_0_N

    XLLP_UINT8_T buffer[1];
    int status;
    const UCHAR GPX_ADDR = 0x66;
    const UCHAR GPX_CAMERA_POWER_DOWN_MASK = 0x04;
    
    //  Set up GPX power_down pins.
    status = OS_I2CMasterReadData(GPX_ADDR, buffer, 1);
    if (XLLP_STATUS_SUCCESS != status) {
        RETAILMSG(1, (TEXT("Failed to read from GPX address.\r\n")));
    } else {
        RETAILMSG(1, (TEXT("GPX value = 0x%02X.\r\n"), buffer[0]));
    }
    
    if (is_on) 
    {
        // Set CIF_PWDN to zero.
        buffer[0] &= ~GPX_CAMERA_POWER_DOWN_MASK;
        status = OS_I2CMasterWriteData(GPX_ADDR, buffer, 1);
        if (XLLP_STATUS_SUCCESS != status) 
        {
            RETAILMSG(1, (TEXT("Failed to write GPX address.\r\n")));
        }        
    } 
    else 
    {
        // Set CIF_PWDN to 1.
        buffer[0] |= GPX_CAMERA_POWER_DOWN_MASK;
        status = OS_I2CMasterWriteData(GPX_ADDR, buffer, 1);
        if (XLLP_STATUS_SUCCESS != status) {
            RETAILMSG(1, (TEXT("Failed to write GPX address.\r\n")));
        }        
    }
    
    //----------------------------------------------------------------------------------------------
    if (is_on)
        Sleep(200);
}

camera_cfg_t* Ov5623::get_camera_cfg(PCS_VIDEOINFOHEADER pCsVideoInfoHdr, ULONG mode)
{
    UINT width = abs(pCsVideoInfoHdr->bmiHeader.biWidth);
    UINT height = abs(pCsVideoInfoHdr->bmiHeader.biHeight);

    if (width == 352 && height == 288)
        return &preview_cfg;
    else if (width == 1280 && height == 960)
        return &still_sxga_cfg;
    else if (width == 1600 && height == 1200)
        return &still_cfg;
    else if (width == 800 && height == 600)
        return &still_svga_cfg;
    else if (width == 2560 && height == 1920)
        return &still_full_cfg;
    else if (width == QXGA_WIDTH && height == QXGA_HEIGHT)
        return &still_ijpg_cfg;
    return 0;
}

void Ov5623::set_frame_format(int format, FrameSize size)
{
    OV5623Reset();
    frame_size = size;
    //XLLP_CI_MP_TIMING timing;
    //int status;
    //XLLP_UINT32_T ovSizeFormat, ovFormat;
    XLLP_UINT32_T width, height;
    XLLP_UINT32_T x[2], y[2];

    switch(size)
    {
    case CIF:
        width = 352;
        height = 288;
        break;
    case SXGA:
        width = 1280;
        height = 960;
        break;
    case FULL:
        width = 2560;
        height = 1920;
        break;
    case QXGA:
        width = QXGA_WIDTH;
        height = QXGA_HEIGHT;
        break;
    case UXGA:
    default:
        width = 1600;
        height = 1200;
        break;
    }

    OV5623SetFormat(width, height, &x[0], &y[0], &x[1], &y[1]);
}


int jpegStreamFlush(void *pStream, void *pStreamHandler, int nAvailBytes, int flag)
{
    IppBitstream *dstBS = (IppBitstream *)pStreamHandler;
    Ipp8u *pData = (Ipp8u *)pStream;

    // FIXME: add out of range handling
    memcpy(dstBS->pBsCurByte, pData, nAvailBytes);
    dstBS->pBsCurByte += nAvailBytes;
    return nAvailBytes;
}

void Ov5623::handle_frame_interrupt(PUCHAR buf,
                                    CS_BITMAPINFOHEADER* info_header,
                                    frame_t* frame)
{
    UINT width = abs(info_header->biWidth);
    UINT height = abs(info_header->biHeight);
    UINT y_size = width * height;
    format_t* format = &frame->list->format;

    if (width != format->width && height != format->height)
    {
        NKDbgPrintfW(L"unexpected frame size!!!");
        return;
    }

    PUCHAR dest[3];



    switch (format->format)
    {
    case XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR:
        //NKDbgPrintfW(L"XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR");

        dest[0] = buf;
        dest[2] = dest[0] + y_size;
        dest[1] = dest[2]  + (y_size >> 2);
        fill_buffer_yv12(dest, frame);
        break;
    case XLLP_CAMERA_IMAGE_FORMAT_RAW10:
        //NKDbgPrintfW(L"XLLP_CAMERA_IMAGE_FORMAT_RAW10");

        if (FOURCC_IJPG == (info_header->biCompression & ~BI_SRCPREROTATE))
        {
            NKDbgPrintfW(L"[Camera] Encoding JPEG width=%d, height=%d", width, height);

            // Add by Leon Cheng
            IppCodecStatus              retCode = IPP_STATUS_NOERR;
            MiscGeneralCallbackTable    *pCBTable = NULL;
            void                        *pJPEGEncState = NULL;
            IppPicture                  srcPicture;
            IppBitstream                dstBitstream;
            IppBitstream                tempBitstream;

            memset(&srcPicture,     0x00,   sizeof(IppPicture));
            memset(&dstBitstream,   0x00,   sizeof(IppBitstream));
            memset(&tempBitstream,  0x00,   sizeof(IppBitstream));

            if (0 != miscInitGeneralCallbackTable(&pCBTable))
            {
                NKDbgPrintfW(L"[Camera] miscInitGeneralCallbackTable() FAILED!!!");

                info_header->biSizeImage = 0;
                return;
            }
            pCBTable->fStreamFlush = (MiscStreamFlushCallback)jpegStreamFlush;

            // Set source picture properties
            srcPicture.picWidth = width;
            srcPicture.picHeight = height;
            srcPicture.picPlaneNum = 3;
            srcPicture.picChannelNum = 3;
            srcPicture.picFormat = JPEG_YUV411;

            // Attach output sample buffer to IppBitstream as output for MVED JPEG enc
            dstBitstream.pBsBuffer = (Ipp8u *)buf;
            dstBitstream.pBsCurByte = dstBitstream.pBsBuffer;
            dstBitstream.bsByteLen = info_header->biSizeImage; // MAX output buffer size
            dstBitstream.bsCurBitOffset = 0;

            //
            tempBitstream.pBsBuffer = (Ipp8u *)malloc(65536 * sizeof(BYTE));
            tempBitstream.pBsCurByte = tempBitstream.pBsBuffer;
            tempBitstream.bsByteLen = 65536;
            tempBitstream.bsCurBitOffset = 0;

            // only SW IPP JPEG Enc need this parameter structure
            IppJPEGEncoderParam         jpegEncParam;

            memset(&jpegEncParam,   0x00,   sizeof(IppJPEGEncoderParam));
            jpegEncParam.nQuality           = 80;
            jpegEncParam.nRestartInterval   = 0;
            jpegEncParam.nJPEGMode          = JPEG_BASELINE;
            jpegEncParam.nSubsampling       = JPEG_411;
            jpegEncParam.pStreamHandler     = &dstBitstream;
            jpegEncParam.nRSTMCUHeight      = RGGB_MCU_HEIGHT;

            //
            retCode = EncoderInitAlloc_JPEG(&jpegEncParam, &srcPicture, &tempBitstream, pCBTable, &pJPEGEncState);
            if (IPP_STATUS_NOERR != retCode) 
            {
                NKDbgPrintfW(L"[Camera] EncoderInitAlloc_JPEG(%d) FAILED!!!", retCode);

                info_header->biSizeImage = 0;
                free(tempBitstream.pBsBuffer);
                miscFreeGeneralCallbackTable(&pCBTable);
                return;
            }

            if (NULL == srcPicture.ppPicPlane[0]) {
                NKDbgPrintfW(L"[Camera] srcPicture.ppPicPlane[0] = NULL");

                info_header->biSizeImage = 0;
                EncoderFree_JPEG(&pJPEGEncState);
                free(tempBitstream.pBsBuffer);
                miscFreeGeneralCallbackTable(&pCBTable);
                return;
            }

            //
            srcPicture.picHeight = (jpegEncParam.nRSTMCUHeight << 4);

            int rggbMCUStep = srcPicture.picWidth * srcPicture.picHeight * 2;
            dest[0] = (PUCHAR)srcPicture.ppPicPlane[0];
            dest[2] = dest[0] + (srcPicture.picWidth * srcPicture.picHeight);
            dest[1] = dest[2] + (srcPicture.picWidth * srcPicture.picHeight >> 2);

            for (unsigned int i = (height/srcPicture.picHeight); i > 0; i --)
            {
                Ipp8u *pSrc = (Ipp8u *)(frame->plane[0].buf.buf + (i-1) * rggbMCUStep);
                int rv = ippi10RGGBtoYCbCr_RotRsz_8u_P3R(pSrc, dest, rpp_mcu);
                if (0 != rv) {
                    NKDbgPrintfW(L"[Camera] ippi10RGGBtoYCbCr_RotRsz_8u_P3R() = %d", rv);
                }

                retCode = Encode_JPEG(&srcPicture, &tempBitstream, pJPEGEncState);
                if ((IPP_STATUS_NEED_INPUT != retCode) && (IPP_STATUS_NOERR != retCode))
                {
                    NKDbgPrintfW(L"[Camera] Encode_JPEG() = %d", retCode);

                    info_header->biSizeImage = (long)(dstBitstream.pBsCurByte - dstBitstream.pBsBuffer);
                    EncoderFree_JPEG(&pJPEGEncState);
                    free(tempBitstream.pBsBuffer);
                    miscFreeGeneralCallbackTable(&pCBTable);
                    return;
                }
            }

            EncoderFree_JPEG(&pJPEGEncState);
            free(tempBitstream.pBsBuffer);
            miscFreeGeneralCallbackTable(&pCBTable);

            // Tell MDD the actual pre-encoded JPEG data size
            info_header->biSizeImage = (long)(dstBitstream.pBsCurByte - dstBitstream.pBsBuffer);
	    } else {
            dest[0] = buf;
            dest[2] = dest[0] + y_size;
            dest[1] = dest[2]  + (y_size >> 2);

            handle_frame_raw10(buf, frame);
	    }
        break;
    default:
        NKDbgPrintfW(L"unsupported frame format %d!!!", format->format);
    }
    return;
}

void Ov5623::handle_frame_raw10(PUCHAR buf, frame_t* frame)
{
    format_t* format = &frame->list->format;
    IppiRawPixProcSpec_P3R* raw_spec;
    UINT y_size = format->width * format->height;

    PUCHAR dest[3];

    dest[0] = buf;
    dest[2] = dest[0] + y_size;
    dest[1] = dest[2]  + (y_size >> 2);

    //NKDbgPrintfW(L"dest plane 0x%08x, 0x%08x, 0x%08x\r\n", dest[0], dest[1], dest[2]);

    // memset(pImage, 0, biSizeImage);
    raw_spec = format->width == 352? rpp_cif : 
               format->width == 1600? rpp_uxga:
               format->width == 2560? rpp_full:
               rpp_qxga;

    if (!rggb2yuv_convert(dest, frame, raw_spec)) {
        NKDbgPrintfW(L"Error: failed to convert format.\r\n");        
    }    
}

static void dump_Ov5623()
{
    unsigned char buf[0x8b];

    OV5623ReadAllRegs(buf, 0x8b);

    for (int i = 0; i < 0x8b; i++)
        NKDbgPrintfW(L"reg %x = %x", i, buf[i]);
}

#define IPP_COE_FLOAT_TO_INT(x)   ((short)((x) * (1 << 8) + 0.5))
static Ipp16s pMatrix[9];
static IppiRawPixProcCfg_P3R  ippiRPPCfg;
static IppiCAMCfg             ippiCAMCfg;

static IppiRawPixProcSpec_P3R *rggb2yuv_init(unsigned int width,
                                      unsigned int height,
                                      float* coe)
{
    IppiRawPixProcSpec_P3R *ippiRPPSpec = NULL;


    for (int i = 0; i < sizeof(pMatrix) / sizeof(pMatrix[0]); i++)
    {
        pMatrix[i] = IPP_COE_FLOAT_TO_INT(coe[i]);
        NKDbgPrintfW(L"matrix[%d] %d", i, pMatrix[i]);
    }
    /* Initializae CAMCfg structure */
    ippiCAMCfg.DPMLen        = 0;
    ippiCAMCfg.DPMOffset.x    = 0;
    ippiCAMCfg.DPMOffset.y    = 0;
    ippiCAMCfg.DPInterp        = ippCameraInterpNearest;
    ippiCAMCfg.GammaFlag    = ippGamPreOneTable;
    ippiCAMCfg.GammaIndex[0]    = 0;
    ippiCAMCfg.pCCMatrix    = pMatrix;
    ippiCAMCfg.pDeadPixMap    = NULL; /* if no dead pixel map */
    ippiCAMCfg.pGammaTable[0]    = NULL;
    ippiCAMCfg.pGammaTable[1]    = NULL;
    ippiCAMCfg.pGammaTable[2]    = NULL; 

    /* Initialize RPPCfg structure */
    ippiRPPCfg.interpolation    = ippCameraInterpBilinear;
    ippiRPPCfg.colorConversion    = ippCameraCscRGGBToYCbCr420;
    ippiRPPCfg.rotation        = ippCameraRotate180;;
    ippiRPPCfg.srcSize.width    = width;
    ippiRPPCfg.srcSize.height    = height;
    ippiRPPCfg.srcStep        = width << 1;

    ippiRPPCfg.dstSize.width    = width;
    ippiRPPCfg.dstSize.height    = height;
    ippiRPPCfg.dstStep[0]    = ippiRPPCfg.dstSize.width;
    ippiRPPCfg.dstStep[1]    = ippiRPPCfg.dstSize.width >> 1;
    ippiRPPCfg.dstStep[2]    = ippiRPPCfg.dstSize.width >> 1;
    
    ippiRPPCfg.bExtendBorder    = ippFalse;
    
    /* call init function */
    if (ippiInitAlloc_10RGGBtoYCbCr_RotRsz_P3R(&ippiCAMCfg, &ippiRPPCfg, &ippiRPPSpec) != ippStsOk)
    {
        NKDbgPrintfW(L"init ipp failed");
        return 0;
    };

    return ippiRPPSpec;
}

static BOOL rggb2yuv_convert(PUCHAR* dest, frame_t* frame, IppiRawPixProcSpec_P3R *ippiRPPSpec)
{
    format_t* format = &frame->list->format;
    Ipp8u *pSrc;
    //Ipp8u *pDst[3];
    int rv;

    pSrc = (Ipp8u*)frame->plane[0].buf.buf;
    //NKDbgPrintfW(L"conver 0x%08x pSrc 0x%08x format 0x%08x", frame, pSrc, format);

    rv = ippi10RGGBtoYCbCr_RotRsz_8u_P3R(pSrc, dest, ippiRPPSpec);

    if (rv != ippStsOk) {
        NKDbgPrintfW(L"Failed to conver to YV12");
        return FALSE;
    }
    return TRUE;
}

static void rggb2yuv_deinit(IppiRawPixProcSpec_P3R *ippiRPPSpec)
{
    if (ippiRPPSpec)
        ippiFree_10RGGBtoYCbCr_RotRsz_P3R(ippiRPPSpec);
}

void Ov5623::write_sensor_reg(UCHAR addr, UCHAR data)
{
    OV5623WriteSensorReg(addr, &data);
}

void Ov5623::set_exposure_gain(USHORT time, USHORT gain)
{
    NKDbgPrintfW(L"exposure %d\n", time);

    UCHAR digi_gain1, digi_gain2;
    get_digi_gain(gain, &digi_gain1, &digi_gain2);

    UCHAR reg_aech = (UCHAR)((time & 0xfc00) >> 10);
    reg_aech |= digi_gain1 << 6;

    write_sensor_reg(0x8a, 0);
    write_sensor_reg(0x45, reg_aech);
    write_sensor_reg(0x4c, digi_gain2 << 5);
    write_sensor_reg(0x10, (UCHAR)((time & 0x3fc) >> 2));
    write_sensor_reg(0x04, (UCHAR)(time & 0x3));
    write_sensor_reg(0x0, get_analog_gain(gain));

}

UCHAR get_analog_gain(USHORT gain)
{
    unsigned int i = 0;

    if (gain > 32)
        gain = 32;

    UCHAR sensor_gain = 0;

    NKDbgPrintfW(L"gain %d\n", gain);

    while(gain > 2 && i < 4)
    {
        gain = gain / 2;
        sensor_gain |= 0x10 << i;
        i++;
    }

    UCHAR small_gain = (UCHAR)(gain * 16) - 16;

    if (small_gain >= 16)
        small_gain = 15;
    sensor_gain |= small_gain;
    
    NKDbgPrintfW(L"sensor gain 0x%x\n", sensor_gain);
    return sensor_gain;
}

UCHAR digi_gain_convert(UCHAR gain)
{
    if (gain <= 1)
        return 0;

    if (gain <= 3)
        return 1;
    else
        return 3;
}

void get_digi_gain(USHORT gain, UCHAR* digi_gain1, UCHAR* digi_gain2)
{
    UCHAR gain1 = 1, gain2 = 1;

    if (gain <= 32)
    {
        digi_gain1 = digi_gain2 = 0;
        return;
    }
        
    gain = gain / 32;

    if (gain > 4)
    {
        gain2 = gain / 4;
        gain1 = 4;
    } else
        gain1 = (UCHAR)gain;

    *digi_gain1 = digi_gain_convert(gain1);
    *digi_gain2 = digi_gain_convert(gain2);

}

int  Ov5623::field_protect()
{
	UCHAR gain_val,aechm_val,aech_val,aecl_val,blueg_val,redg_val;
	OV5623_MODE mode;
	
	NKDbgPrintfW(L"ov5623hw_field_protect()\r\n");

	field_pclk = OV5623CalculatePClk(ov5623_interface.clock);

	/* let the sensor work on proper mode */
    switch(frame_size)
    {
        case CIF:
            mode = OV5623_D1MD;
            break;
        case SXGA:
            mode = OV5623_SXGA;
            break;
        case UXGA:
        case QXGA:
        case FULL:
        default:
            mode = OV5623_FULL;
            break;
    }

    switch(mode)
    {
        case OV5623_D1MD:
            field_max_pixel = 1300;
            field_max_line = 617;
            break;
        case OV5623_SXGA:
            field_max_pixel = 1640;
            field_max_line = 977;
            break;
        case OV5623_FULL:
            field_max_pixel = 3252;
            field_max_line = 1968;
            break;
        default:
            NKDbgPrintfW(L"manual_exp_gain:Wrong still image size.\r\n");
            break;
	}

	OV5623ReadSensorReg(OV5623_GAIN, &gain_val);
	NKDbgPrintfW(L"OV5623_GAIN = %2x\n",gain_val);
	OS_Delay(5);
	OV5623ReadSensorReg(OV5623_REG45, &aechm_val);
	NKDbgPrintfW(L"OV5623_REG45 = %2x\n",aechm_val);
	OS_Delay(5);
	OV5623ReadSensorReg(OV5623_AEC, &aech_val);
	NKDbgPrintfW(L"OV5623_AEC = %2x\n",aech_val);
	OS_Delay(5);
	OV5623ReadSensorReg(OV5623_REG04, &aecl_val);
	NKDbgPrintfW(L"OV5623_REG04 = %2x\n",aecl_val);
	OS_Delay(5);
	OV5623ReadSensorReg(OV5623_BLUE, &blueg_val);
	NKDbgPrintfW(L"OV5623_BLUE = %2x\n",blueg_val);
	OS_Delay(5);
	OV5623ReadSensorReg(OV5623_RED, &redg_val);
	NKDbgPrintfW(L"OV5623_RED = %2x\n",redg_val);
	OS_Delay(5);

	field_gain = gain_val;
	field_exp = (((ULONG)(aechm_val & 0x3f)) << 11) +	\
		(((ULONG)(aech_val)) << 3) + (ULONG)(aecl_val & 0x07);
	field_blueg = blueg_val;
	field_redg = redg_val;

	NKDbgPrintfW(L"field_gain = %d\n", field_gain);
	NKDbgPrintfW(L"field_exp = %d\n", field_exp);

	return 0;
	
} 

int Ov5623::calculate_exp()
{
	UCHAR aecl_val,aech_val,aechm_val;
	ULONG capture_exp, preview_exp;
	ULONG capture_max_line, preview_max_line;
	ULONG capture_max_pixel, preview_max_pixel;
	ULONG capture_pixel_clk, preview_pixel_clk;
	ULONG capture_exp_gain;
	ULONG time_unit;
	ULONG capture_gain;
	UCHAR  tmp_gain;
	OV5623_MODE mode;

	NKDbgPrintfW(L"ov5623hw_calculate_exp().\r\n");

	/* let the sensor work on proper mode */
    switch(frame_size)
    {
        case CIF:
            mode = OV5623_D1MD;
            break;
        case SXGA:
            mode = OV5623_SXGA;
            break;
        case UXGA:
        case QXGA:
        case FULL:
        default:
            mode = OV5623_FULL;
            break;
    }

	switch(mode)
	{
	case OV5623_D1MD:
		capture_max_pixel = 1300; 
		capture_max_line = 617;
		break;
	case OV5623_SXGA:
		capture_max_pixel = 1640;
		capture_max_line = 977;
		break;
	case OV5623_FULL:
		capture_max_pixel = 3252;
		capture_max_line = 1968;
		break;
	default:
		NKDbgPrintfW(L"manual_exp_gain:Wrong still image size.\r\n");
		break;
	}

	capture_pixel_clk = OV5623CalculatePClk(ov5623_interface.clock); 

	preview_exp = field_exp;
	preview_max_line = field_max_line;
	preview_max_pixel = field_max_pixel;
	preview_pixel_clk = field_pclk;

	
	NKDbgPrintfW(L"preview_exp = %d\r\n",preview_exp);
	NKDbgPrintfW(L"capture_pixel_clk = %d\r\n",capture_pixel_clk);
	NKDbgPrintfW(L"preview_max_pixel = %d\r\n",preview_max_pixel);
	NKDbgPrintfW(L"preview_pixel_clk = %d\r\n",preview_pixel_clk);
	NKDbgPrintfW(L"capture_max_pixel = %d\r\n",capture_max_pixel);

	capture_exp = preview_exp * capture_pixel_clk *	\
		preview_max_pixel /( preview_pixel_clk * capture_max_pixel);
	NKDbgPrintfW(L"capture_exp = %d\r\n",capture_exp);

	/*convert reg 0x00 to field_gain * 16*/
	capture_gain =(field_gain & 0x0f) + 16;


	if (field_gain & 0x10)
		capture_gain = capture_gain << 1;
	if (field_gain & 0x20)
		capture_gain = capture_gain << 1;
	if (field_gain & 0x40)
		capture_gain = capture_gain << 1;
	if (field_gain & 0x80)
		capture_gain = capture_gain << 1;

	capture_exp_gain = capture_exp * capture_gain;
	NKDbgPrintfW(L"OV5623: capture_exp_gain = %08x\r\n", capture_exp_gain);


	// calculate banding filter value
	// 1/100s for 50Hz, 1/120s for 60Hz*/
	time_unit = capture_pixel_clk *10000 / capture_max_pixel / 100;
	NKDbgPrintfW(L"time_unit = %d\r\n",time_unit);


	/* calculate field_gain, exposure*/

	if (capture_exp_gain < (capture_max_line * 16))
	{
		capture_exp = capture_exp_gain / 16;
		
		if (capture_exp > time_unit) 
		{
			capture_exp /= time_unit;
			capture_exp *= time_unit;
		}
		NKDbgPrintfW(L"capture_exp = %d\r\n",capture_exp);
	}
	else
	{
		capture_exp = capture_max_line;
		NKDbgPrintfW(L"capture_exp = %d\r\n",capture_exp);
	}


	capture_gain = (capture_exp_gain*100 + 50)/capture_exp/100;
	NKDbgPrintfW(L"capture_gain = %d\r\n",capture_gain);

	// calculate reg0x00 from field_gain * 16.

	tmp_gain = 0;
	if (capture_gain > 31)
	{
		tmp_gain |= 0x10;
		capture_gain = capture_gain >> 1;
	}	
	if (capture_gain > 31)
	{
		tmp_gain |= 0x20;
		capture_gain = capture_gain >> 1;
	}	
	if (capture_gain > 31)
	{
		tmp_gain |= 0x40;
		capture_gain = capture_gain >> 1;
	}	
	if (capture_gain > 31)
	{
		tmp_gain |= 0x80;
		capture_gain = capture_gain >> 1;
	}	

	if (capture_gain > 16)
		tmp_gain |= ((capture_gain -16) & 0x0f);
	
	capture_gain = tmp_gain;


	/*set values to reg*/
	OV5623ReadSensorReg(OV5623_REG04, &aecl_val);
	aecl_val &= 0xf8;
	aecl_val |= (UCHAR)(capture_exp & 0x07);

	OV5623ReadSensorReg(OV5623_REG45, &aechm_val);
	aechm_val &= 0xc0;
	aechm_val |= (UCHAR)((capture_exp & 0x1f800) >> 11);

	aech_val = (UCHAR)((capture_exp & 0x7f8) >> 3);


	OV5623WriteSensorReg(OV5623_GAIN, 	&tmp_gain);
	OV5623WriteSensorReg(OV5623_BLUE,	&field_blueg);
	OV5623WriteSensorReg(OV5623_RED,  	&field_redg);
	OV5623WriteSensorReg(OV5623_REG45, 	&aechm_val);
	OV5623WriteSensorReg(OV5623_AEC, 	&aech_val);
	OV5623WriteSensorReg(OV5623_REG04, 	&aecl_val);
	
	NKDbgPrintfW(L"OV5623_GAIN = %2x\r\n", tmp_gain);
	NKDbgPrintfW(L"OV5623_REG45 = %2x\r\n", aechm_val);
	NKDbgPrintfW(L"OV5623_AEC = %2x\r\n", aech_val);
	NKDbgPrintfW(L"OV5623_REG04 = %2x\r\n", aecl_val);	

	return 0;		
}
